"""
排队状态分析与集群识别模块
"""

import numpy as np
import logging
from typing import List, Dict, Any
from scipy.cluster.hierarchy import linkage, fcluster
from scipy.spatial.distance import pdist, squareform

class QueueAnalyzer:
    """排队分析器"""
    
    def __init__(self, config):
        self.config = config
        self.logger = logging.getLogger(__name__)
        
        # 车辆状态记录
        self.vehicle_states = {}  # track_id -> state_history
        
    def analyze(self, vehicles):
        """分析排队状态"""
        # 更新车辆状态
        self.update_vehicle_states(vehicles)
        
        # 识别排队车辆
        queuing_vehicles = self.identify_queuing_vehicles(vehicles)
        
        # 空间聚类
        queue_clusters = self.spatial_clustering(queuing_vehicles)
        
        # 分析排队状态
        queue_states = self.analyze_queue_states(queue_clusters)
        
        return queue_clusters, queue_states
    
    def update_vehicle_states(self, vehicles):
        """更新车辆状态记录"""
        current_time = len(self.vehicle_states)  # 简化的时间戳
        
        for vehicle in vehicles:
            track_id = vehicle['track_id']
            
            if track_id not in self.vehicle_states:
                self.vehicle_states[track_id] = []
            
            # 计算当前状态
            state = self.calculate_vehicle_state(vehicle)
            self.vehicle_states[track_id].append(state)
            
            # 只保留最近的状态记录
            if len(self.vehicle_states[track_id]) > 100:  # 保留最近100个状态
                self.vehicle_states[track_id].pop(0)
    
    def calculate_vehicle_state(self, vehicle):
        """计算车辆状态"""
        trajectory = vehicle.get('trajectory', [])
        
        if len(trajectory) < 2:
            return {
                'speed': 0.0,
                'is_moving': False,
                'position': [0, 0] if not trajectory else trajectory[-1]
            }
        
        # 计算瞬时速度
        recent_positions = trajectory[-5:]  # 最近5个位置
        if len(recent_positions) < 2:
            speed = 0.0
        else:
            displacements = []
            for i in range(1, len(recent_positions)):
                disp = np.linalg.norm(
                    np.array(recent_positions[i]) - np.array(recent_positions[i-1])
                )
                displacements.append(disp)
            speed = np.mean(displacements) if displacements else 0.0
        
        # 判断是否在运动
        is_moving = speed > self.config.speed_threshold
        
        return {
            'speed': speed,
            'is_moving': is_moving,
            'position': trajectory[-1] if trajectory else [0, 0],
            'timestamp': len(self.vehicle_states.get(vehicle['track_id'], []))
        }
    
    def identify_queuing_vehicles(self, vehicles):
        """识别排队车辆"""
        queuing_vehicles = []
        
        for vehicle in vehicles:
            track_id = vehicle['track_id']
            state_history = self.vehicle_states.get(track_id, [])
            
            if len(state_history) < self.config.duration_threshold:
                continue
            
            # 检查最近的状态
            recent_states = state_history[-self.config.duration_threshold:]
            stopped_duration = sum(1 for state in recent_states 
                                 if not state['is_moving'])
            
            # 判断是否满足排队条件
            stopped_ratio = stopped_duration / len(recent_states)
            if stopped_ratio >= 0.8:  # 80%的时间处于停止状态
                vehicle['queue_state'] = 'queuing'
                vehicle['stopped_duration'] = stopped_duration
                queuing_vehicles.append(vehicle)
            else:
                vehicle['queue_state'] = 'moving'
                
        return queuing_vehicles
    
    def spatial_clustering(self, vehicles):
        """空间聚类分析"""
        if len(vehicles) < 2:
            return [vehicles] if vehicles else []
        
        # 提取车辆位置
        positions = []
        vehicle_indices = []
        
        for i, vehicle in enumerate(vehicles):
            state_history = self.vehicle_states.get(vehicle['track_id'], [])
            if state_history:
                latest_position = state_history[-1]['position']
                positions.append(latest_position)
                vehicle_indices.append(i)
        
        if len(positions) < 2:
            return [vehicles]
        
        positions = np.array(positions)
        
        # 计算自定义距离矩阵
        distance_matrix = self.calculate_custom_distance(positions)
        
        # 层次聚类
        try:
            Z = linkage(squareform(distance_matrix), method='average')
            clusters = fcluster(Z, self.config.cluster_eps, criterion='distance')
            
            # 组织聚类结果
            cluster_dict = {}
            for i, cluster_id in enumerate(clusters):
                if cluster_id not in cluster_dict:
                    cluster_dict[cluster_id] = []
                cluster_dict[cluster_id].append(vehicles[vehicle_indices[i]])
            
            return list(cluster_dict.values())
            
        except Exception as e:
            self.logger.warning(f"聚类错误: {str(e)}")
            return [vehicles]
    
    def calculate_custom_distance(self, positions):
        """计算自定义距离矩阵"""
        n = len(positions)
        distance_matrix = np.zeros((n, n))
        
        # 估计车道方向（使用主成分分析）
        if n >= 2:
            centered = positions - np.mean(positions, axis=0)
            cov_matrix = np.cov(centered.T)
            eigenvalues, eigenvectors = np.linalg.eig(cov_matrix)
            lane_direction = eigenvectors[:, np.argmax(eigenvalues)]
        else:
            lane_direction = np.array([1, 0])  # 默认方向
        
        for i in range(n):
            for j in range(i + 1, n):
                # 向量差
                vector_ij = positions[j] - positions[i]
                
                # 纵向距离（沿车道方向）
                longitudinal_dist = abs(np.dot(vector_ij, lane_direction))
                
                # 横向距离（垂直车道方向）
                lateral_dir = np.array([-lane_direction[1], lane_direction[0]])
                lateral_dist = abs(np.dot(vector_ij, lateral_dir))
                
                # 欧氏距离
                euclidean_dist = np.linalg.norm(vector_ij)
                
                # 综合距离
                combined_dist = (
                    self.config.longitudinal_weight * longitudinal_dist +
                    self.config.lateral_weight * lateral_dist +
                    self.config.euclidean_weight * euclidean_dist
                )
                
                distance_matrix[i, j] = combined_dist
                distance_matrix[j, i] = combined_dist
        
        return distance_matrix
    
    def analyze_queue_states(self, clusters):
        """分析排队状态"""
        queue_states = {}
        
        for i, cluster in enumerate(clusters):
            if not cluster:
                continue
                
            # 计算集群统计信息
            positions = []
            speeds = []
            
            for vehicle in cluster:
                state_history = self.vehicle_states.get(vehicle['track_id'], [])
                if state_history:
                    latest_state = state_history[-1]
                    positions.append(latest_state['position'])
                    speeds.append(latest_state['speed'])
            
            if not positions:
                continue
                
            positions = np.array(positions)
            speeds = np.array(speeds)
            
            # 集群特征
            cluster_center = np.mean(positions, axis=0)
            cluster_size = len(cluster)
            avg_speed = np.mean(speeds) if len(speeds) > 0 else 0.0
            spatial_extent = np.max(pdist(positions)) if len(positions) > 1 else 0.0
            
            queue_states[f'cluster_{i}'] = {
                'center': cluster_center.tolist(),
                'size': cluster_size,
                'avg_speed': avg_speed,
                'spatial_extent': spatial_extent,
                'vehicle_ids': [v['track_id'] for v in cluster],
                'state': 'stable' if avg_speed < 0.1 else 'unstable'
            }
        
        return queue_states
    
    def estimate_lane_direction(self, positions):
        """估计车道方向"""
        if len(positions) < 2:
            return np.array([1, 0])  # 默认方向
        
        # 使用主成分分析估计主要方向
        centered = positions - np.mean(positions, axis=0)
        cov_matrix = np.cov(centered.T)
        
        try:
            eigenvalues, eigenvectors = np.linalg.eig(cov_matrix)
            main_direction = eigenvectors[:, np.argmax(eigenvalues)]
            return main_direction
        except:
            return np.array([1, 0])